
#include "qyQuat.h"
#include "qyMatrix4.h"

//-----------------------------------------------------------------------
void Quat::ToMatrix4( Matrix4 &mat ) const
{
	Vector3 v2( 2.0f * x, 2.0f * y, 2.0f * z );

	double xx=1-v2.x*x;		double yy=v2.y*y;		double xw=v2.x*w;
	double xy=v2.y*x;		double yz=v2.z*y;		double yw=v2.y*w;
	double xz=v2.z*x;		double zz=v2.z*z;		double zw=v2.z*w;
	mat[0][0] = float(1-yy-zz);	mat[1][0] = float(xy-zw);	mat[2][0] = float(xz+yw);
	mat[0][1] = float(xy+zw);	mat[1][1] = float(xx-zz);	mat[2][1] = float(yz-xw);
	mat[0][2] = float(xz-yw);	mat[1][2] = float(yz+xw);	mat[2][2] = float(xx-yy);
}

//-----------------------------------------------------------------------
void Quat::FromMatrix4( const Matrix4 &m )
{
	/*
	w = sqrt( 1.0f + m[0][0] + m[0][0] + m[0][0] ) / 2.0f;
	
	float w4 = (4.0f * w );
	x = ( m[1][2] - m[2][1] ) / w4 ;
	y = ( m[2][0] - m[0][2] ) / w4 ;
	z = ( m[0][1] - m[1][0] ) / w4 ;
	*/
	float s,p,tr = m[0][0] + m[1][1]+ m[2][2];

	//check the diagonal
	if(tr > 0.0) {
		s=sqrt(tr+1.0f);	assert(s); p=0.5f/s;
		Set( s*0.5f, (m[1][2]-m[2][1])*p, (m[2][0]-m[0][2])*p, (m[0][1]-m[1][0])*p );
		return;
	}
	//diagonal is negative. now we have to find the biggest element on the diagonal
	//check if "m00" is the biggest element
	if	( (m[0][0]>=m[1][1]) && (m[0][0]>=m[2][2]) ) {
		s=sqrt(m[0][0]-m[1][1]-m[2][2]+1.0f);	assert(s);	p=0.5f/s;
		Set( (m[1][2]-m[2][1])*p, s*0.5f, (m[0][1]+m[1][0])*p, (m[0][2]+m[2][0])*p );
		return;
	}
	//check if "m11" is the biggest element
	if	( (m[1][1]>=m[0][0]) && (m[1][1]>=m[2][2]) ) {
		s=sqrt(m[1][1]-m[2][2]-m[0][0]+1.0f);	assert(s); p=0.5f/s;
		Set( (m[2][0]-m[0][2])*p, (m[1][0]+m[0][1])*p, s*0.5f, (m[1][2]+m[2][1])*p );
		return;
	}
	//check if "m22" is the biggest element
	if	( (m[2][2]>=m[0][0]) && (m[2][2]>=m[1][1]) ) {
		s=sqrt(m[2][2]-m[0][0]-m[1][1]+1.0f);	assert(s); p=0.5f/s;
		Set( (m[0][1]-m[1][0])*p, (m[2][0]+m[0][2])*p, (m[2][1]+m[1][2])*p, s*0.5f );
		return;
	}
}

